using System;
using System.Collections.Generic;
using System.Text;
using DotNetMatrix;

namespace Blimpbots.Recognition.Algorithms
{
    public class AngularKalmanFilter : KalmanFilter
    {
        private double _last_t;
        double _stDev;

        public AngularKalmanFilter(double stDev, double init_stDev, double ang, double t)
        {
            _F = GeneralMatrix.Identity(3, 3);

            _stDev = stDev;

            _last_t = t;

            // Note that we have no certainty about the position
            _P = GeneralMatrix.Identity(3, 3).MultiplyEquals(init_stDev * init_stDev);

            _x = new GeneralMatrix(new double[] { Math.Cos(ang), Math.Sin(ang), 0}, 3);

        }

        public double[] Update(double z, double meas_stDev, double t)
        {
            #region Convert Z to Matrix
            GeneralMatrix zMat = new GeneralMatrix(new double[] { Math.Cos(z), Math.Sin(z) }, 2);
            #endregion

            double dT = t - _last_t;
            _last_t = t;

            #region Prepare Q
            _Q = CalcQ(dT);
            #endregion

            #region Prepare F

            // Position estimate is the same as before
            //   plus velocity times dT
            double thta = Theta(_x.Array[1][0], _x.Array[0][0]);
            double w = _x.Array[2][0];

            double eps = 0.0001;

            if (w > -eps && w < eps)
            {
                if (w > 0) w = eps;
                else w = -eps;
            }

            _F.Array[0][2] = (Math.Cos(w + thta) - Math.Cos(thta)) / w;
            _F.Array[1][2] = (Math.Sin(w + thta) - Math.Sin(thta)) / w;
            
            #endregion

            #region Predict and Prepare H

            GeneralMatrix B = new GeneralMatrix(2, 3);
            B.Array[0][0] = 1;
            B.Array[1][1] = 1;
            _H = B;

            GeneralMatrix _xHat = Predict();

            double cosVal = _xHat.Array[0][0];
            double shouldBe = Math.Cos(w + thta);

            if (cosVal == shouldBe) cosVal = 1;

            #endregion

            #region Prepare R
            _R = GeneralMatrix.Identity(2, 2);
            _R.Array[0][0] = meas_stDev * meas_stDev;
            _R.Array[1][1] = meas_stDev * meas_stDev;
            #endregion

            #region Update
            GeneralMatrix x = Update(zMat);
            #endregion

            #region Convert x to Angle            

            double result = Theta(x.Array[1][0], x.Array[0][0]);

            #endregion

            return new double[] { result , x.Array[2][0] };
        }

        private GeneralMatrix CalcQ(double dT)
        {
            double thta = Theta(_x.Array[1][0], _x.Array[0][0]);
            GeneralMatrix Q = GeneralMatrix.Identity(3, 3);

            Q.Array[0][0] = _stDev * (dT * dT) / 2;
            Q.Array[1][1] = _stDev * (dT * dT) / 2;
            Q.Array[2][2] = _stDev * dT; 
            
            return Q;
        }

        private double Theta(double sin, double cos)
        {
            if (cos > -0.5 && cos < 0.5)
            {
                Console.WriteLine("good");
            }

            if (cos >  1.0) cos =  1.0;
            if (cos < -1.0) cos = -1.0;

            double theta = Math.Acos(cos);
            if (sin > 0)
                return theta;
            else
                return (Math.PI * 2) - theta;




        } 
    
    }


}
